'''
Created on Aug 6, 2019

@author: modys
'''
import numpy as np
import matplotlib.pyplot as plt

class Vehicle(object):
    '''
    classdocs
    '''


    def __init__(self):
        '''
        Constructor
        '''
        # ==================================
        #  Parameters
        # ==================================
    
        #Throttle to engine torque
        self.a_0 = 400
        self.a_1 = 0.1
        self.a_2 = -0.0002
        
        # Gear ratio, effective radius, mass + inertia
        self.GR = 0.35
        self.r_e = 0.3
        self.J_e = 10
        self.m = 2000
        self.g = 9.81
        
        # Aerodynamic and friction coefficients
        self.c_a = 1.36
        self.c_r1 = 0.01
        
        # Tire force 
        self.c = 10000
        self.F_max = 10000
        
        # State variables
        self.x = 0
        self.v = 5
        self.a = 0
        self.w_e = 100
        self.w_e_dot = 0
        
        self.sample_time = 0.01
        
    def reset(self):
        # reset state variables
        self.x = 0
        self.v = 5
        self.a = 0
        self.w_e = 100
        self.w_e_dot = 0    
        
    def step(self, throttle, alpha):
        # ==================================
        #  Implement vehicle model here
        # ==================================
        w_w = self.GR * self.w_e;
        s = (w_w * self.r_e - self.v) / self.v;
        
        if(abs(s) <1):
            F_x = self.c *s;
        else:
            F_x = self.F_max;
        F_g = self.m * self.g*np.sin(alpha);
        R_x = self.c_r1 *self.v;
        F_aero = self.a * (self.v**2);
        F_load = F_aero - R_x - F_g;
        
        T_e = throttle * ( self.a_0 + (self.a_1*self.w_e) + ( self.a_2 * (self.w_e**2) ) );
        
        self.a = (F_x - F_load)/self.m;
        self.v = self.v + self.a * self.sample_time;
        self.x = self.x + self.v *self.sample_time;
        
        self.w_e_dot = T_e - (self.GR *self.r_e*F_load);
        self.w_e = self.w_e + self.w_e_dot * self.sample_time;
        
            
         